/*
 * top.cc
 *
 *  Created on: Jul 21, 2016
 *      Author: loywong
 */
#include <ti/sysbios/knl/Task.h>
#include <ti/sysbios/hal/Hwi.h>
#include <ti/sysbios/knl/Mailbox.h>
#include <xdc/runtime/Error.h>
#include <xdc/runtime/System.h>
#include <ti/sysbios/BIOS.h>
#include <xdc/runtime/Timestamp.h>

#include <inc/hw_sysctl.h>
#include <inc/hw_pwm.h>
#include <inc/hw_qei.h>
#include <inc/hw_gpio.h>
#include <inc/hw_memmap.h>
#include <inc/hw_types.h>
#include <inc/hw_ints.h>
#include <linemouse_chassis.h>
#include <ti/drivers/GPIO.h>
#include <math.h>

#include "TskTop.h"
#include "../TskMotor/TskMotor.h"
#include "../TskIr/TskIr.h"
#include "../TskLed/TskLed.h"
#include "../TskDebug/TskDebug.h"
#include "../TskRun/TskRun.h"
#include "DbgUart.h"
#include <string.h>
#include <stdio.h>

namespace TskTop
{

const int tskPrio = 4;
const int tskStkSize = 2560;
Task_Params tskParams;
Task_Handle tsk;
//Error_Block eb;

char dbgStr[80];

volatile MouseMode::ModeType Mode;

void encImuMonitor()
{
    int i = 0;

    TskMotor::MotorMsg::MsgType motMsg;
//    TskIr::IrMsg::MsgType irMsg;

    Task_sleep(1000);

    motMsg = TskMotor::MotorMsg::DisableMotors;
    Mailbox_post(TskMotor::MbCmd, &motMsg, BIOS_NO_WAIT);
//    irMsg = TskIr::IrMsg::DisableEmitt;
//    Mailbox_post(TskIr::MbCmd, &irMsg, BIOS_NO_WAIT);
    motMsg = TskMotor::MotorMsg::EnableAcqZeros;
    Mailbox_post(TskMotor::MbCmd, &motMsg, BIOS_NO_WAIT);

    Task_sleep(1000);// wait for getting imu zeros & getting ir zeros

    motMsg = TskMotor::MotorMsg::EnableAccl;
    Mailbox_post(TskMotor::MbCmd, &motMsg, BIOS_NO_WAIT);
    motMsg = TskMotor::MotorMsg::EnableGyro;
    Mailbox_post(TskMotor::MbCmd, &motMsg, BIOS_NO_WAIT);
//    irMsg = TskIr::IrMsg::EnableEmitt;
//    Mailbox_post(TskIr::MbCmd, &irMsg, BIOS_NO_WAIT);

    while(true)
    {
        if(!(i++ & 0xf))
        {
            DbgUartPutLine( "\n GYZero\t  GyroZ\t AYZero\t  AcclY\t EncVel\t KalVel\t DisAcc\n", true);
        }
        sprintf(dbgStr, "%7.3f\t%7.3f\t%7.3f\t%7.3f\t%7.4f\t%7.4f\t%8.3f\n",
                TskMotor::GyroZZero, TskMotor::AV,
                TskMotor::AcclYZero, TskMotor::AcclY,
                TskMotor::EncVel, TskMotor::LV,
                TskMotor::DistanceAcc);
        DbgUartPutLine(dbgStr, true);
        Task_sleep(500);
    }
}

void irMonitor()
{
    int i = 0;

    TskMotor::MotorMsg::MsgType motMsg;
//    TskIr::IrMsg::MsgType irMsg;

    Task_sleep(1000);

    motMsg = TskMotor::MotorMsg::DisableMotors;
    Mailbox_post(TskMotor::MbCmd, &motMsg, BIOS_NO_WAIT);
//    irMsg = TskIr::IrMsg::DisableEmitt;
//    Mailbox_post(TskIr::MbCmd, &irMsg, BIOS_NO_WAIT);
    motMsg = TskMotor::MotorMsg::EnableAcqZeros;
    Mailbox_post(TskMotor::MbCmd, &motMsg, BIOS_NO_WAIT);

    Task_sleep(1000);// wait for getting imu zeros & getting ir zeros

    motMsg = TskMotor::MotorMsg::DisableAcqZeros;
    Mailbox_post(TskMotor::MbCmd, &motMsg, BIOS_NO_WAIT);
    motMsg = TskMotor::MotorMsg::EnableAccl;
    Mailbox_post(TskMotor::MbCmd, &motMsg, BIOS_NO_WAIT);
    motMsg = TskMotor::MotorMsg::EnableGyro;
    Mailbox_post(TskMotor::MbCmd, &motMsg, BIOS_NO_WAIT);
//    irMsg = TskIr::IrMsg::EnableEmitt;
//    Mailbox_post(TskIr::MbCmd, &irMsg, BIOS_NO_WAIT);

    while(true)
    {
        if(!(i++ & 0xf))
        {
            DbgUartPutLine( "\n LWay:b\t Left:b\tCentr:b\tRight:b\t RWay:b\n", true);
        }
        sprintf(dbgStr, "%5d:%1d\t%5d:%1d\t%5d:%1d\t%5d:%1d\t%5d:%1d\n",
                TskIr::IrInts.lway, TskIr::IrBins.lway,
                TskIr::IrInts.left, TskIr::IrBins.left,
                TskIr::IrInts.center, TskIr::IrBins.center,
                TskIr::IrInts.right, TskIr::IrBins.right,
                TskIr::IrInts.rway, TskIr::IrBins.rway
        );
        DbgUartPutLine(dbgStr, true);
        Task_sleep(500);
    }

}

void SetLeds(LedColor::Type led0, LedColor::Type led1)
{
    unsigned char rgb0, rgb1;
    rgb0 = (unsigned char)led0;
    rgb1 = (unsigned char)led1;
    GPIO_write(DBMOUSE_LED0_R, (rgb0 & 0x1) ? DBMOUSE_LED_ON : DBMOUSE_LED_OFF);
    GPIO_write(DBMOUSE_LED0_G, (rgb0 & 0x2) ? DBMOUSE_LED_ON : DBMOUSE_LED_OFF);
    GPIO_write(DBMOUSE_LED0_B, (rgb0 & 0x4) ? DBMOUSE_LED_ON : DBMOUSE_LED_OFF);
    GPIO_write(DBMOUSE_LED1_R, (rgb1 & 0x1) ? DBMOUSE_LED_ON : DBMOUSE_LED_OFF);
    GPIO_write(DBMOUSE_LED1_G, (rgb1 & 0x2) ? DBMOUSE_LED_ON : DBMOUSE_LED_OFF);
    GPIO_write(DBMOUSE_LED1_B, (rgb1 & 0x4) ? DBMOUSE_LED_ON : DBMOUSE_LED_OFF);
}

void modeName(MouseMode::ModeType mode, char *str)
{
    switch(mode)
    {
    case MouseMode::Idle:
        sprintf(str, "Mode Idle.\n");
        break;
    case MouseMode::EncImuMonitor:
        sprintf(str, "Mode Enc & Imu Monitor.\n\tTouch Ir to start\n");
        break;
    case MouseMode::IrMonitor:
        sprintf(str, "Mode Ir Monitor.\n\tTouch Ir to start\n");
        break;
    case MouseMode::Undef2:
        sprintf(str, "Mode Ir Correction.\n\tTouch Ir to start.\n");
        break;
    case MouseMode::MotionCorrection:
        sprintf(str, "Mode MotionCorr.\n");
        break;
    case MouseMode::ListenUartCmd:
        sprintf(str, "Mode Listen Uart Command.\n");
        break;
    case MouseMode::Undef0:
        sprintf(str, "Mode Undef0.\n");
        break;
    case MouseMode::Undef1:
        sprintf(str, "Mode Undef1.\n");
        break;
    default:
        sprintf(str, "Undefined mode.\n");
        break;
    }
}

inline void dbgPutModeName(MouseMode::ModeType mode)
{
    modeName(mode, dbgStr);
    DbgUartPutLine(dbgStr, true);
}

void task(UArg arg0, UArg arg1)
{
////////////////////////////////////////////////
//    int ts[8], i = 0;
//    float a = 3.212221f;
//    float b = -1.354415;
//    float ii = 1399.f;
//    float c;
//    xdc_runtime_Types_FreqHz freq;
//    ts[i] = Timestamp_get32();
//    c = a * b;
//    ts[i] = Timestamp_get32() - ts[i];
//    i++;
//
//    ts[i] = Timestamp_get32();
//    c = a / b;
//    ts[i] = Timestamp_get32() - ts[i];
//    i++;
//
//    ts[i] = Timestamp_get32();
//    c = powf(a, b);
//    ts[i] = Timestamp_get32() - ts[i];
//    i++;
//
//    ts[i] = Timestamp_get32();
//    c = expf(a);
//    ts[i] = Timestamp_get32() - ts[i];
//    i++;
//
//    ts[i] = Timestamp_get32();
//    c = logf(a);
//    ts[i] = Timestamp_get32() - ts[i];
//    i++;
//
//    ts[i] = Timestamp_get32();
//    c = powf(10, b);
//    ts[i] = Timestamp_get32() - ts[i];
//    i++;
//
//    ts[i] = Timestamp_get32();
//    c = logf(b) / logf(a);
//    ts[i] = Timestamp_get32() - ts[i];
//    i++;
//
//    b = 1.f / b;
//    ts[i] = Timestamp_get32();
//    c = expf((logf(ii) - a) * b);
//    ts[i] = Timestamp_get32() - ts[i];
//    i++;
//
//    Timestamp_getFreq(&freq);
////////////////////////////////////////////////

//    TskIr::IrMsg::MsgType irMsg;

    InitDbgUart();

    //SetLeds(LedColor::Red, LedColor::Red);
    //Task_sleep(1000);
    //SetLeds(LedColor::Green, LedColor::Green);
    //Task_sleep(1000);
    SetLeds(LedColor::Blue, LedColor::Blue);
    Task_sleep(1000);

    DbgUartPutLine("Hello from dbmouse!\n", true);

    TskMotor::Init();
    TskIr::Init();
    TskLed::Init();
    TskDebug::Init();
    TskRun::Init();

//    irMsg = TskIr::IrMsg::EnableEmitt;
//    Mailbox_post(TskIr::MbCmd, &irMsg, BIOS_NO_WAIT);

    Task_sleep(500);
    DbgUartPutLine("Roll wheel to change mode...\n", true);

    MouseMode::ModeType lastMode = Mode = MouseMode::Idle;
    //SetLeds((LedColor::Type)Mode, (LedColor::Type)Mode);
    dbgPutModeName(Mode);

    TskMotor::EncLAct = false;
    TskMotor::EncRAct = false;


    while(true)
    {
        //Mode = (MouseMode::ModeType)(lroundf(TskMotor::DistanceAcc * 100.f) & 15);
    	//Mode = MouseMode::EncImuMonitor;
    	Mode = MouseMode::Idle;

        if(Mode != lastMode)
        {
            //SetLeds((LedColor::Type)Mode, (LedColor::Type)Mode);
            dbgPutModeName(Mode);
        }
        lastMode = Mode;
        if(TskMotor::EncRAct)
        {
            TskMotor::EncRAct = false;
            switch(Mode)
            {
            case MouseMode::Idle:
                break;
            case MouseMode::EncImuMonitor:
                encImuMonitor();
                break;
            case MouseMode::IrMonitor:
                irMonitor();
                break;
            default:
                break;
            }
        }

        Task_sleep(50);
    }

//    while(true)
//    {
//        Task_sleep(500);
////        System_printf("%d\n", lroundf(TskMotor::DistanceAcc * 10000.f));
////        System_flush();
//        sprintf(dbgStr, "%4d\t%4d\t%4d\t%4d\n",
//                TskIr::IrInts.fl,
//                TskIr::IrInts.sl,
//                TskIr::IrInts.sr,
//                TskIr::IrInts.fr);
//        DbgUartPutLine(dbgStr);
//    }
}

void Init()
{
//    Error_init(&eb);
    Task_Params_init(&tskParams);
    tskParams.priority = tskPrio;
    tskParams.stackSize = tskStkSize;
    tsk = Task_create((Task_FuncPtr)task, &tskParams, NULL);
    if(tsk == NULL)
    {
        System_abort("Task Top failed");
    }
}

}


